/*******************************************************************************
 *
 *      Filename: test_quat.cpp
 *   Description: Unit test for quat class
 *        Author: [kb]ypp
 *    Created at: November 6, 2005
 * Last modified: 
 *
 ******************************************************************************/

#include "../Quaternion.h"
#include "../Vector3.h"
#include "../Matrix3x3.h"
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
using namespace boost::unit_test;
using boost::test_toolbox::close_at_tolerance;
#include <cmath>
#include <iostream>
using namespace std;

const float TOLERANCE = 0.0001;

ostream& operator<<( ostream &os, const quat &q )
{
	return os << "( " << q.w << "; " << q.x
			  << "; " << q.y << "; " << q.z << " )";
}

ostream& operator<<( ostream &os, const mat3 &m )
{
	return os << "( " << m.m11 << ' ' << m.m12 << ' ' << m.m13 << "; "
					  << m.m21 << ' ' << m.m22 << ' ' << m.m23 << "; "
					  << m.m31 << ' ' << m.m32 << ' ' << m.m33
			  << " )";
}


void constructors_test()
{
	// Use the default constructor:
	quat df;
	
	// Test the constructor with 4 float`s:
	quat q( 1.0, -9.91674e-05, 0.0, 9.91674e-05 );
	BOOST_CHECK_CLOSE( q.w, 1.0f, TOLERANCE );
	BOOST_CHECK_CLOSE( q.x, -9.91674e-05f, TOLERANCE );
	BOOST_CHECK_CLOSE( q.y, 0.0f, TOLERANCE );
	BOOST_CHECK_CLOSE( q.z, 9.91674e-05f, TOLERANCE );
	
	// Test the copy-constructor:
	quat qCopy( q );
	BOOST_CHECK_CLOSE( qCopy.w, q.w, TOLERANCE );
	BOOST_CHECK_CLOSE( qCopy.x, q.x, TOLERANCE );
	BOOST_CHECK_CLOSE( qCopy.y, q.y, TOLERANCE );
	BOOST_CHECK_CLOSE( qCopy.z, q.z, TOLERANCE );
	quat dfCopy( df );
	BOOST_CHECK_CLOSE( dfCopy.w, df.w, TOLERANCE );
	BOOST_CHECK_CLOSE( dfCopy.x, df.x, TOLERANCE );
	BOOST_CHECK_CLOSE( dfCopy.y, df.y, TOLERANCE );
	BOOST_CHECK_CLOSE( dfCopy.z, df.z, TOLERANCE );
}

void assignment_test()
{
	// Test operator=()
	const quat q( 1.0, -9.91674e-05, 0.0, 9.91674e-05 );
	quat qAssign;
	qAssign = q;
	BOOST_CHECK_CLOSE( qAssign.w, q.w, TOLERANCE );
	BOOST_CHECK_CLOSE( qAssign.x, q.x, TOLERANCE );
	BOOST_CHECK_CLOSE( qAssign.y, q.y, TOLERANCE );
	BOOST_CHECK_CLOSE( qAssign.z, q.z, TOLERANCE );
}

void arithmetic_test()
{
	const quat q1( 1.0, 1.0125e-05, 2.14954e-05, 4.55625e-05 );
	const quat q2( -0.999999, 0.0, -0.0004536, 0.0010125 );
	
	// Test operator*()
	BOOST_CHECK_EQUAL( q1 * q2,
		quat( -0.999999, -1.00826e-05, -0.000475106, 0.000966933 ) );
	BOOST_CHECK_EQUAL( q2 * q1,
		quat( -0.999999, -1.01674e-05, -0.000475085, 0.000966942 ) );
}

void comparison_test()
{
	quat q1( 1.0, 1.0125e-05, 2.14954e-05, 4.55625e-05 );
	quat q2( 1.0, 1.0125e-05, 2.14954e-05, 4.55625e-05 );
	
	// Test operator==()
	BOOST_CHECK_EQUAL( q1, quat( 1.0, 1.0125e-05, 2.14954e-05, 4.55625e-05 ) );
	BOOST_CHECK_EQUAL( quat( 1.0, 1.0125e-05, 2.14954e-05, 4.55625e-05 ), q1 );
	BOOST_CHECK_EQUAL( q1, q2 );
	BOOST_CHECK_EQUAL( q2, q1 );
	
	q1.z = 2.0;
	
	// Test operator!=()
	BOOST_CHECK( q1 != quat( 1.0, 1.0125e-05, 2.14954e-05, 4.55625e-05 ) );
	BOOST_CHECK( quat( 1.0, 1.0125e-05, 2.14954e-05, 4.55625e-05 ) != q1 );
	BOOST_CHECK( q1 != q2 );
	BOOST_CHECK( q2 != q1 );
	
	BOOST_CHECK_EQUAL( q1, quat( 1.0, 1.0125e-05, 2.14954e-05, 2.0 ) );
}

void other_test()
{
	const quat q( 1.0, -9.91674e-05, 0.0, 9.91674e-05 );
	const quat q1( 1.0, 1.0125e-05, 2.14954e-05, 4.55625e-05 );
	const quat q2( -0.999999, 0.0, -0.0004536, 0.0010125 );
	
	// Test normalize()
	quat unit_q( 1.0, -13564.0, 0.2687, 0.0 );
	float mag = sqrt( unit_q.w * unit_q.w +
					  unit_q.x * unit_q.x +
					  unit_q.y * unit_q.y +
					  unit_q.z * unit_q.z );
	BOOST_CHECK( !( fabs( mag - 1.0 ) < TOLERANCE ) );
	unit_q.normalize();
	mag = sqrt( unit_q.w * unit_q.w +
				unit_q.x * unit_q.x +
				unit_q.y * unit_q.y +
				unit_q.z * unit_q.z );
	BOOST_CHECK_CLOSE( mag, 1.0f, TOLERANCE );
	quat( 0.0, 0.0, 0.0, 0.0 ).normalize(); // Log warning message expected!
	
	// Test toMatrix()
	BOOST_CHECK_EQUAL( q1.toMatrix(), mat3( 1.0, -9.11246e-05, 4.29917e-05,
											9.11254e-05, 1.0, -2.0248e-05,
											-4.29899e-05, 2.0252e-05, 1.0 ) );
	BOOST_CHECK_EQUAL( q2.toMatrix(), mat3( 0.999998, 0.002025, 0.000907199,
											-0.002025, 0.999998, -9.1854e-07,
											-0.000907199, -9.1854e-07, 1.0 ) );
	
	// Test conj()
	BOOST_CHECK_CLOSE( conj( q ).w,  q.w, TOLERANCE );
	BOOST_CHECK_CLOSE( conj( q ).x, -q.x, TOLERANCE );
	BOOST_CHECK_CLOSE( conj( q ).y, -q.y, TOLERANCE );
	BOOST_CHECK_CLOSE( conj( q ).z, -q.z, TOLERANCE );
	BOOST_CHECK_CLOSE( conj( q1 ).w,  q1.w, TOLERANCE );
	BOOST_CHECK_CLOSE( conj( q1 ).x, -q1.x, TOLERANCE );
	BOOST_CHECK_CLOSE( conj( q1 ).y, -q1.y, TOLERANCE );
	BOOST_CHECK_CLOSE( conj( q1 ).z, -q1.z, TOLERANCE );
	BOOST_CHECK_CLOSE( conj( q2 ).w,  q2.w, TOLERANCE );
	BOOST_CHECK_CLOSE( conj( q2 ).x, -q2.x, TOLERANCE );
	BOOST_CHECK_CLOSE( conj( q2 ).y, -q2.y, TOLERANCE );
	BOOST_CHECK_CLOSE( conj( q2 ).z, -q2.z, TOLERANCE );
	
	// Test slerp()
	BOOST_CHECK_EQUAL( slerp( q1, q2, 0.0 ), q1 );
	BOOST_CHECK_EQUAL( slerp( q1, q2, 0.128 ),
		quat( 1.0, 8.829e-06, 7.68048e-05, -8.98696e-05 ) );
	BOOST_CHECK_EQUAL( slerp( q1, q2, 0.5 ),
		quat( 1.0, 5.0625e-06, 0.000237548, -0.000483469 ) );
	BOOST_CHECK_EQUAL( slerp( q1, q2, 0.679 ),
		quat( 1.0, 3.25013e-06, 0.000314894, -0.000672862 ) );
	BOOST_CHECK_EQUAL( slerp( q1, q2, 1.0 ), q2 );
}

void statics_test()
{
	vec3 axis( 0.0, 0.0789026, -0.996882 );
	float rad = 0.789;
	BOOST_CHECK_EQUAL( quat::rotateX( rad ),
					   quat( 0.923189, 0.384347, 0.0, 0.0 ) );
	BOOST_CHECK_EQUAL( quat::rotateY( rad ),
					   quat( 0.923189, 0.0, 0.384347, 0.0 ) );
	BOOST_CHECK_EQUAL( quat::rotateZ( rad ),
					   quat( 0.923189, 0.0, 0.0, 0.384347 ) );
	BOOST_CHECK_EQUAL( quat::rotateAxis( axis, rad ),
					   quat( 0.923189, 0.00000, 0.0303260, -0.3831480 ) );
	
	rad = -1.21;
	BOOST_CHECK_EQUAL( quat::rotateX( rad ).toMatrix(), mat3::rotateX( rad ) );
	BOOST_CHECK_EQUAL( quat::rotateY( rad ).toMatrix(), mat3::rotateY( rad ) );
	BOOST_CHECK_EQUAL( quat::rotateZ( rad ).toMatrix(), mat3::rotateZ( rad ) );
	BOOST_CHECK_EQUAL( quat::rotateAxis( axis, rad ).toMatrix(),
					   mat3::rotateAxis( axis, rad ) );
}

test_suite* init_unit_test_suite( int argc, char* argv[] )
{
	test_suite *test = BOOST_TEST_SUITE( "Unit tests for quat class" );
	
	test->add( BOOST_TEST_CASE( &constructors_test ) );
	test->add( BOOST_TEST_CASE( &assignment_test ) );
	test->add( BOOST_TEST_CASE( &arithmetic_test ) );
	test->add( BOOST_TEST_CASE( &comparison_test ) );
	test->add( BOOST_TEST_CASE( &other_test ) );
	test->add( BOOST_TEST_CASE( &statics_test ) );
	
	return test;
}
